function [newCheckArcs newP updateOrigIndex newIndex fixedIndex fixedOffPoints] = checkResult(x,y,z,P,approxP,origIndex,sites,triangles, checkArcs, n, N,gridStructure, numAddPoints)

time = [0:0.1:1]';
newP = [];
idxCnt = 0;
updateOrigIndex = [];
fixedIndex = [];
fixedOffPoints = [];
margin = 80;

newCheckArcs = [];

for i=1:n-1
    
    disp(['Checking collision for arc : ' num2str(i) '/' num2str(n-1)]);
    flagNewPointsAdded = 0; % flag to check if new point is added to an arc
    prevArcNewPointsAdded = 1;
    
    C(i,:) = [x((i-1)*N + 1) y((i-1)*N + 1) z((i-1)*N + 1)];
    F = ((1-time).^2)*approxP(i,:) + (2*(1-time).*time)*C(i,:) + (time.^2)*approxP(i+1,:);
    
    newP = [newP; P(i,:)];
    idxCnt = idxCnt + 1;
    
    if(find(origIndex==i))
        updateOrigIndex = [updateOrigIndex; idxCnt];
    end

    % check if sample trajectory is underneath terrain for all
    % subsamples of trajectory
    if(isempty(find(checkArcs==i,1))) 
        % means that this arc is ok. don't tuch this curve for next
        % optimization
        fixedIndex = [fixedIndex; idxCnt];
        fixedOffPoints = [fixedOffPoints; C(i,:)];
        continue; % skip arcs that were not violated before 
    end; 

    testIndex = 1;
    for j=1:length(time)-1
        triangleIndex = searchTriangle(F(j,:),sites,triangles,testIndex,gridStructure);
        if(triangleIndex~=0)
            nodeA = sites(triangles(triangleIndex,1),:);
            nodeB = sites(triangles(triangleIndex,2),:);
            nodeC = sites(triangles(triangleIndex,3),:);
            if(checkSafety(F(j,:), nodeA, nodeB, nodeC)<0) 
                
                % if not safe make control point nearby
                % find vertex in neighbor that has the highest height to
                % satisfy safety

                % pick n vertices that are closest to current point
                for k=1:length(sites)
                    distVertex(k) = norm(F(j,[1 2]) - sites(k,[1 2]));
                end
                [sortVal sortIndex] = sort(distVertex);

                validPoint = 0; iter = 15;
                shortestVertices = sortIndex(1:iter);

                [sortHeight sortSiteIndex] = sort(sites(shortestVertices,3));

                % add points 
                while(~validPoint && iter > 0)
                    
                    if(numAddPoints == 1)
                        newPoint = sites(shortestVertices(sortSiteIndex(iter)),:);
                        newPoint(3) = newPoint(3) + margin;
                        
                        iter = iter - 1;
                        
                        % if point already exists don't add
                        for k=1:size(P,1)
                            if(sum(P(k,[1 2]) == newPoint(1,[1 2]) ))
                                validPoint = 0; break;
                            end 
                            if(k==size(P,1)) validPoint = 1; end % point doesn't exist
                        end
                    else % numAddPoints = 2
                        newPoint = [sites(shortestVertices(sortSiteIndex(iter)),:); sites(shortestVertices(sortSiteIndex(iter-1)),:)];
                        newPoint(:,3)  = newPoint(:,3) + margin*ones(numAddPoints,1);
                        
                        iter = iter - 2;
                        
                        % if point already exists don't add
                        for k=1:size(P,1)
                            if(sum(P(k,[1 2]) == newPoint(1,[1 2]) | P(k,[1 2]) == newPoint(2,[1 2]) ))
                                validPoint = 0; break;
                            end 
                            if(k==size(P,1)) validPoint = 1; end % point doesn't exist
                        end
                    end

                end

                if(validPoint)
                    if(numAddPoints == 1)
                        newP = [newP; newPoint];  
                        newCheckArcs = [newCheckArcs; [idxCnt:idxCnt+1]'];
                    else
                        newP = [newP(1:end-1,:); newPoint(1,:); newP(end,:); newPoint(2,:)];
                        newCheckArcs = [newCheckArcs; [idxCnt-2:idxCnt+1]']; % add 4. 2 for each added points
                    end

                    idxCnt = idxCnt + numAddPoints;
                    
                    flagNewPointsAdded = 1;
                    
                else
                    disp('no points added even though collision');
                end

                break; % add only one control point per arc
     
                
            end
            testIndex = triangleIndex;

        end
        
    end
     
    if(numAddPoints == 1)
        if(flagNewPointsAdded == 0)
            fixedIndex = [fixedIndex; idxCnt];
            fixedOffPoints = [fixedOffPoints; C(i,:)];     
        end
    else
        if(flagNewPointsAdded==0 && prevArcNewPointsAdded==0 && i~=n-1)
            fixedIndex = [fixedIndex; idxCnt - 1];
            fixedOffPoints = [fixedOffPoints; C(i-1,:)];

            prevArcNewPointsAdded = 0;
        elseif(flagNewPointsAdded==0 && prevArcNewPointsAdded)
            prevArcNewPointsAdded = 0;
        else % flagNewPointsAdded ==1
            prevArcNewPointsAdded = 1;
        end
    end    
end

newP = [newP; P(n,:)];
updateOrigIndex = [updateOrigIndex; idxCnt+1];

tempIndex = 1:size(newP,1);
for i=1:length(updateOrigIndex)
    tempIndex(updateOrigIndex(i)) = 0;
end
newIndex = tempIndex(tempIndex > 0);
